/*
以下例子在SMS40BL中测试通过，舵机出厂速度单位为0.732rpm，舵机机运行速度V=80
如果使用的出厂速度单位是0.0146rpm，则速度改为V=4000，延时公式T=[(P1-P0)/V]*1000+[V/(A*100)]*1000
*/
#include <ros/ros.h>
// This header is provided by ros, but here it seems to be unnecessary.
// #include <serial/serial.h>
#include <iostream>
#include "scservo/SCServo.h"

SMSBL sm;

int main(int argc, char** argv)
{
// Initialize the node.
	ros::init(argc, argv, "serial_port");
// Define node handle.
	ros::NodeHandle n;
// Set sleep time of a loop.
	ros::Rate loop_rate(0.1);

// Define a SMSBL servo.
	SMSBL sm;
// The baudrate is always 1000000, not 115200. The name of serial port is always /dev/ttyUSB0.
	if(sm.begin(1000000, "/dev/ttyUSB0"))
	{
// if the serial port opend, throw out an INFO.
		ROS_INFO("Port \"dev/ttyUSB0\" is opened!");
	}


// Write position.
	    sm.RegWritePosEx(0, 0, 300, 1000);//舵机(ID1)以最高速度V=80(50*80步/秒)，加速度A=100(100*100步/秒^2)，运行至P1=4095位置
		sm.RegWritePosEx(1, 100, 300, 1000);//舵机(ID2)以最高速度V=80(50*80步/秒)，加速度A=100(100*100步/秒^2)，运行至P1=4095位置
		sm.RegWritePosEx(2, 2850, 300, 1000);//舵机(ID2)以最高速度V=80(50*80步/秒)，加速度A=100(100*100步/秒^2)，运行至P1=4095位置
		sm.RegWritePosEx(3, 0, 300, 1000);//舵机(ID2)以最高速度V=80(50*80步/秒)，加速度A=100(100*100步/秒^2)，运行至P1=4095位置
		sm.RegWritePosEx(4, 3359, 300, 1000);//舵机(ID2)以最高速度V=80(50*80步/秒)，加速度A=100(100*100步/秒^2)，运行至P1=4095位置
		sm.RegWritePosEx(5, 2048, 300, 1000);//舵机(ID2)以最高速度V=80(50*80步/秒)，加速度A=100(100*100步/秒^2)，运行至P1=4095位置
		sm.RegWriteAction();

		ROS_INFO("ID: %d\tPosition: %d", 1, 3072);
		loop_rate.sleep();
	

// Close the serial port.
	sm.end();

	return 0;
}










